
#include <unitree_camera_sdk/UnitreeCameraSDK.hpp>
#include <unistd.h>

using namespace std;

class Cameras
{
public:
    Cameras(int source):
        // cam("../stereo_camera_config_0.yaml") ///< init camera by device node number
        cam(source)
    {
        int deviceNode = source; ///< default 0 -> /dev/video0
        cv::Size frameSize(1856, 800); ///< default frame size 1856x800
        int fps = 30; ///< default camera fps: 30

        if(!cam.isOpened())   ///< get camera open state
            exit(EXIT_FAILURE);
        cam.setRawFrameSize(frameSize); ///< set camera frame size
        cam.setRawFrameRate(fps);       ///< set camera camera fps
        cam.setRectFrameSize(cv::Size(frameSize.width >> 2, frameSize.height >> 1)); ///< set camera rectify frame size
        cam.startCapture(); ///< disable image h264 encoding and share memory sharing
        std::cout << "Camera class finished construct" << endl;
        usleep(500000);
    }

    void getframe();
    void save(void (*processing)(cv::Mat), int index);
    void show(void (*processing)(cv::Mat));
    void stop();

    UnitreeCamera cam;
    cv::Mat frame;
};


void Cameras::getframe()
{
    if(cam.isOpened())
    {
        cv::Mat right;
        cam.getRectStereoFrame(frame,right); ///< get rectify left,right frame  
        cv::flip(frame,frame, -1);
        return;
    }
    else
    {
        std::cout << "can not open camera.";
        exit(-1);
    }
}


void Cameras::save(void (*processing)(cv::Mat), int index=-1)
{
    if(processing)
    {
        processing(frame);
    }

    if(index != -1)
    {
        // char ad[300] = {0};
        // sprintf(ad, "./%d.jpg", index);
        cv::imwrite(std::to_string(index)+".jpg", frame);
    }
    else
    {
        cv::imwrite("tmp.jpg", frame);
    }
    return;
}


void Cameras::show(void (*processing)(cv::Mat))
{
    while(cam.isOpened()){
        cv::Mat left,right;
        if(!cam.getRectStereoFrame(left,right)){ ///< get rectify left,right frame  
            usleep(1000);
            continue;
        }
        if(processing)
        {
            processing(left);
            processing(right);
        }

        cv::Mat stereo;
        // cv::flip(left,left, -1);
        // cv::flip(right,right, -1);
        cv::hconcat(left, right, stereo); 
        cv::flip(stereo,stereo, -1);
        cv::imshow("Longlat_Rect", stereo);
        char key = cv::waitKey(10);
        if(key == 27) // press ESC key
           break;
    }
}


void Cameras::stop()
{
    cam.stopCapture(); ///< stop camera capturing
}
